using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;

using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  public class DistanceSensor : Sensor
  {
    MapWithLandmarks.LandmarkIDs landmarkID;
    double headingDirection;
    double angleOfView;

    public DistanceSensor()
    {
      this.landmarkID = MapWithLandmarks.LandmarkIDs.centerLeftFlag;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 2.0;
    }

    public DistanceSensor(MapWithLandmarks.LandmarkIDs landmarkID)
    {
      this.landmarkID = landmarkID;
      this.headingDirection = 0;
      this.angleOfView = Math.PI / 3.0;
    }

    public DistanceSensor(MapWithLandmarks.LandmarkIDs landmarkID, double headingDirection, double angleOfView)
    {
      this.landmarkID = landmarkID;
      this.headingDirection = headingDirection;
      this.angleOfView = angleOfView;
    }


    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      bool limitView = DebugKeyTool.IsActive("_measurement:limited view");
      double sigma = Globals.distanceMeasurementSigma;
      double gaussianError = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double systematicError = 20.0;
      switch (Globals.selfLocatorProperties.SensorNoise)
      {
        case NoiseType.equal: break; //todo
        case NoiseType.noNoise: gaussianError = 0; systematicError = 0; break;
        case NoiseType.gaussianAndSystematic: break;
        case NoiseType.gaussianNoSystematic: systematicError = 0; break;
        case NoiseType.noGaussianButSystematic: gaussianError = 0; break;
      }
      DistanceMeasurement measurement = (DistanceMeasurement)PredictPerfect2(worldState, map, limitView);

      if (measurement == null)
        return null;
      else
        return new DistanceMeasurement(measurement.Distance + gaussianError + systematicError, this);
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      return PredictPerfect2(worldState, map, false);
    }

    private Measurement PredictPerfect2(WorldState worldState, Map map, bool limitView)
    {
      double distance = 0;

      double x_robot = 0;
      double y_robot = 0;
      double theta_robot = 0;
      double x_landmark = 0;
      double y_landmark = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (worldState is WorldStatePose2D)
      {
        x_robot = (worldState as WorldStatePose2D).Pose.translation.x;
        y_robot = (worldState as WorldStatePose2D).Pose.translation.y;
        theta_robot = (worldState as WorldStatePose2D).Pose.rotation;
      }

      if (map is MapWithLandmarks)
      {
        x_landmark = (map as MapWithLandmarks).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithLandmarks).YPositionOfLandmark[(int)landmarkID];
      }
      if (map is MapWithSingleLandmark)
      {
        x_landmark = (map as MapWithSingleLandmark).XPositionOfLandmark[(int)landmarkID];
        y_landmark = (map as MapWithSingleLandmark).YPositionOfLandmark[(int)landmarkID];
      }

      distance = Math.Sqrt(Math.Pow(y_landmark - y_robot,2) + Math.Pow(x_landmark - x_robot, 2) );

      double angle = Math.Atan2(y_landmark - y_robot, x_landmark - x_robot) - theta_robot;
      if (Math.Abs(MathTools.Normalize(angle - headingDirection)) < angleOfView / 2.0 || !limitView)
        return new DistanceMeasurement(distance, this);
      else
        return null;

    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      double expectedMeasurement = (PredictPerfect(worldState, map) as DistanceMeasurement).Distance;
      double conductedMeasurement = (measurement as DistanceMeasurement).Distance;
      return Globals.Gaussian(conductedMeasurement, Globals.distanceMeasurementSigma, expectedMeasurement);
    }

    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      double distance = (measurement as DistanceMeasurement).Distance;
      double startX = 0;
      double startY = 0;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      if (worldState is WorldStatePose2D)
      {
        startX = (worldState as WorldStatePose2D).Pose.translation.x;
        startY = (worldState as WorldStatePose2D).Pose.translation.y;
      }
      Color color = Color.FromArgb((int)(255.0), (int)(200.0 * shade), (int)(200.0 * shade));
      double width = 5;
      if (shade == 0.9999) { color = Color.Gray; width = 1; }
      DebugDrawingTool.CreateCircle(drawingID, new Vector2(startX, startY), distance, width, color);
      DebugDrawingTool.CreateLine(drawingID, new Vector2(startX - 15, startY), new Vector2(startX + 15, startY), width, color);
      DebugDrawingTool.CreateLine(drawingID, new Vector2(startX, startY - 15), new Vector2(startX, startY + 15), width, color);
    }

    public override string ToString()
    {
      return this.GetType().Name + ";" + landmarkID.ToString();
    }
  }
}
